//
// GPSMapEdit
// (c) Konstantin Galichsky (kg@geopainting.com), 2002-2007
//
// Math algorithms.
//

# include "StdAfx.h"
# include "MathAlgo.h"
# include "PtAlgo.h"

float Polynom (size_t _cN, const float * _pK, float _x) {
	float fRet = _pK [_cN];
	for (int n = _cN - 1; n >= 0; -- n) {
		fRet *= _x;
		fRet += _pK [n];
	}
	return fRet;
}

	// Solve system of 3 linear equations:  B + Ax*x[i] + Ay*y[i] = screen[i]
	// to get values of B, Ax and Ay from known (x,y)->screen.
bool Solve3x3 (const point_t _points [3], const float _lScreen [3], float & _Ax, float & _Ay, float & _B) {
	const float K = (_points [2].y - _points [0].y)*(_points [1].x - _points [0].x) - (_points [1].y - _points [0].y)*(_points [2].x - _points [0].x);
	if (K == 0)
		// All 3 points belong to a line.
		return false;

	_Ay = ((_lScreen [2] - _lScreen [0])*(_points [1].x - _points [0].x) - (_lScreen [1] - _lScreen [0])*(_points [2].x - _points [0].x))/K;

	if (_points [1].x != _points [0].x)
		_Ax = (_lScreen [1] - _lScreen [0] - _Ay*(_points [1].y - _points [0].y))/(_points [1].x - _points [0].x);
	else if (_points [2].x != _points [0].x)
		_Ax = (_lScreen [2] - _lScreen [0] - _Ay*(_points [2].y - _points [0].y))/(_points [2].x - _points [0].x);
	else
		return false;

	_B = _lScreen [0] - _Ax*_points [0].x - _Ay*_points [0].y;

	return true;
}

	// Solve system of 4 linear equations:  B + Ax*x[i] + Ay*y[i] + D*x[i]*y[i] = screen[i]
	// to get values of B, Ax and Ay, D from known (x,y)->screen.
bool Solve4x4_XY (const point_t _points [4], const float _lScreen [4], float & _Ax, float & _Ay, float & _B, float & _D) {
	const float & x1 = _points [0].x, & y1 = _points [0].y;
	const float & x2 = _points [1].x, & y2 = _points [1].y;
	const float & x3 = _points [2].x, & y3 = _points [2].y;
	const float & x4 = _points [3].x, & y4 = _points [3].y;

	const float l1 = _lScreen [0];
	const float l2 = _lScreen [1];
	const float l3 = _lScreen [2];
	const float l4 = _lScreen [3];

	const float K3 = (x2 - x1)*(y3 - y1) - (x3 - x1)*(y2 - y1);
	const float K4 = (x2 - x1)*(y4 - y1) - (x4 - x1)*(y2 - y1);
	const float M3 = (x2 - x1)*(x3*y3 - x1*y1) - (x3 - x1)*(x2*y2 - x1*y1);
	const float M4 = (x2 - x1)*(x4*y4 - x1*y1) - (x4 - x1)*(x2*y2 - x1*y1);
	const float N3 = (x2 - x1)*(l3 - l1) - (x3 - x1)*(l2 - l1);
	const float N4 = (x2 - x1)*(l4 - l1) - (x4 - x1)*(l2 - l1);

	// D.
	if (K3*M4 == K4*M3)
		return false;
	_D = (K3*N4 - N3*K4)/(K3*M4 - K4*M3);

	// Ay.
	if (K3 != 0)
		_Ay = (N3 - _D*M3)/K3;
	else
		_Ay = (N4 - _D*M4)/K4;

	// Ax.
	if (x2 != x1)
		_Ax = ((l2 - l1) - _Ay*(y2 - y1) - _D*(x2*y2 - x1*y1))/(x2 - x1);
	else if (x3 != x1)
		_Ax = ((l3 - l1) - _Ay*(y3 - y1) - _D*(x3*y3 - x1*y1))/(x3 - x1);
	else if (x4 != x1)
		_Ax = ((l4 - l1) - _Ay*(y4 - y1) - _D*(x4*y4 - x1*y1))/(x4 - x1);
	else
		return false;

	// B.
	_B = l1 - _Ax*x1 - _Ay*y1 - _D*x1*y1;

	return true;
}

	// Solve system of 4 linear equations:  B + Ax*x[i] + Ay*y[i] + D*x[i]*x[i] = screen[i]
	// to get values of B, Ax and Ay, D from known (x,y)->screen.
bool Solve4x4_XX (const point_t _points [4], const float _lScreen [4], float & _Ax, float & _Ay, float & _B, float & _D) {
	const float & x1 = _points [0].x, & y1 = _points [0].y;
	const float & x2 = _points [1].x, & y2 = _points [1].y;
	const float & x3 = _points [2].x, & y3 = _points [2].y;
	const float & x4 = _points [3].x, & y4 = _points [3].y;

	const float & l1 = _lScreen [0];
	const float & l2 = _lScreen [1];
	const float & l3 = _lScreen [2];
	const float & l4 = _lScreen [3];

	const float K3 = (x2 - x1)*(y3 - y1) - (x3 - x1)*(y2 - y1);
	const float K4 = (x2 - x1)*(y4 - y1) - (x4 - x1)*(y2 - y1);
	const float M3 = (x2 - x1)*(x3*x3 - x1*x1) - (x3 - x1)*(x2*x2 - x1*x1);
	const float M4 = (x2 - x1)*(x4*x4 - x1*x1) - (x4 - x1)*(x2*x2 - x1*x1);
	const float N3 = (x2 - x1)*(l3 - l1) - (x3 - x1)*(l2 - l1);
	const float N4 = (x2 - x1)*(l4 - l1) - (x4 - x1)*(l2 - l1);

	// D.
	if (K3*M4 == K4*M3)
		return false;
	_D = (K3*N4 - N3*K4)/(K3*M4 - K4*M3);

	// Ay.
	if (K3 != 0)
		_Ay = (N3 - _D*M3)/K3;
	else
		_Ay = (N4 - _D*M4)/K4;

	// Ax.
	if (x2 != x1)
		_Ax = ((l2 - l1) - _Ay*(y2 - y1) - _D*(x2*x2 - x1*x1))/(x2 - x1);
	else if (x3 != x1)
		_Ax = ((l3 - l1) - _Ay*(y3 - y1) - _D*(x3*x3 - x1*x1))/(x3 - x1);
	else
		_Ax = ((l4 - l1) - _Ay*(y4 - y1) - _D*(x4*x4 - x1*x1))/(x4 - x1);

	// B.
	_B = l1 - _Ax*x1 - _Ay*y1 - _D*x1*x1;

	return true;
}

//////////////////////////////////////////////////////

CLinearEquationSystem::CLinearEquationSystem (size_t _N) :
    N (_N),
    m_pMatrix (new CString [_N]),
    m_cErrorIndex (_N)
{
    for (size_t i = 0; i < N; i ++) {
        m_pMatrix [i].pCells = new double [N];
        m_pMatrix [i].cIndex = i;
    }
}

CLinearEquationSystem::~CLinearEquationSystem () {
    for (size_t i = 0; i < N; i ++)
        delete [] m_pMatrix [i].pCells;
    delete [] m_pMatrix;
}

void CLinearEquationSystem::SwapStrings (size_t _j1, size_t _j2) {
    const CString tmp = m_pMatrix [_j1];
    m_pMatrix [_j1] = m_pMatrix [_j2];
    m_pMatrix [_j2] = tmp;
}

void CLinearEquationSystem::SetZero () {
    for (size_t i = 0; i < N; i ++) {
        for (size_t j = 0; j < N; j ++)
             Matrix (i, j) = 0;
        FreeVector (i) = 0;
    }
}

bool CLinearEquationSystem::Solve (double _fMinValue) {
    m_cErrorIndex = N;

    // The first pass: convert the matrix to upper triangle one.
    size_t k;
    for (k = 0; k < N; ++ k) {
        // Find max element in the k-th column.
        double fMaxValue = 0;
        size_t cMaxIndex = k;
        size_t j;
        for (j = k; j < N; ++ j) {
            const double fValue = Matrix (k, j);
            if (::fabs (fValue) > ::fabs (fMaxValue)) {
                fMaxValue = fValue;
                cMaxIndex = j;
            }
        }

        if (::fabs (fMaxValue) < ::fabs (_fMinValue)) {
            // Report about degeneration error.
            m_cErrorIndex = m_pMatrix [cMaxIndex].cIndex;
            return true;
        }

        if (cMaxIndex != k)
            SwapStrings (cMaxIndex, k);

        // Now, Matrix(k,k) is max cell in k-th right bottom submatrix.
        // Make first column of this submatrix zero (except first element).
        // NOTE: all the cells are untouched indeed as they will not make
        // influence on the further calculations.
        for (j = k + 1; j < N; ++ j) {
            const double a = Matrix (k, j)/fMaxValue;
            for (size_t i = k + 1; i < N; ++ i)
                Matrix (i, j) -= Matrix (i, k)*a;
            FreeVector (j) -= FreeVector (k)*a;
        }
    }

    // The second pass (backward).
    for (k = N - 1; int (k) >= 0; -- k) {
        for (size_t i = k + 1; i < N; ++ i)
            FreeVector (k) -= FreeVector (i)*Matrix (i, k);
        FreeVector (k) /= Matrix (k, k);
    }

    return false;
}
